/*
Copyright (c) 2010-2014, Jian Li - Guidance and Control Lab - Tsinghua University, Automation Department
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
    * Neither the name of the Universite de Sherbrooke nor the
      names of its contributors may be used to endorse or promote products
      derived from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/


#ifndef SENSORDATA_H_
#define SENSORDATA_H_

#include "rgbdslamexp.h"
#include "transform.h"
#include "cameramodel.h"

#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>

namespace rgbdslam
{

/**
 * An id is automatically generated if id=0.
 */
class RGBDSLAM_EXP SensorData
{
public:
	// empty constructor
	SensorData();

	// Appearance-only constructor
	SensorData(
			const cv::Mat & image,
			int id = 0,
			double stamp = 0.0,
			const cv::Mat & userData = cv::Mat());

	// Mono constructor
	SensorData(
			const cv::Mat & image,
			const CameraModel & cameraModel,
			int id = 0,
			double stamp = 0.0,
			const cv::Mat & userData = cv::Mat());

	// RGB-D constructor
	SensorData(
			const cv::Mat & rgb,
			const cv::Mat & depth,
			const CameraModel & cameraModel,
			int id = 0,
			double stamp = 0.0,
			const cv::Mat & userData = cv::Mat());
	
	virtual ~SensorData() {}

	bool isValid() const {
		return !(id_ == 0 &&
			stamp_ == 0.0 &&
			rgb_raw_.empty() &&
			rgb_compressed_.empty() &&
			depth_raw_.empty() &&
			depth_compressed_.empty() &&
			camera_models_.size() == 0 &&
			keypoints_.size() == 0 &&
			descriptors_.empty());
	}

	//getters 
	int get_id() const {return id_;}
	
	double get_stamp() const {return stamp_;}
	
	const cv::Mat & get_rgb_compressed() const {return rgb_compressed_;}
	
	const cv::Mat & get_depth_compressed() const {return depth_compressed_;}
	
	const cv::Mat & get_rgb_raw() const {return rgb_raw_;}
	
	const cv::Mat & get_depth_raw() const {return depth_raw_;}
	
	//cv::Mat get_depth_raw() const {return depth_raw_.type()!=CV_8UC1?depth_raw_:cv::Mat();}
	
	const std::vector<CameraModel> & get_camera_models() const {return camera_models_;}
	
	const std::vector<cv::KeyPoint> & get_key_points() const {return keypoints_;}

	const cv::Mat & get_descriptors() const {return descriptors_;}
	
	const Transform & get_ground_truth() const {return groundTruth_;}
	
	//setters
	void set_id(int id) {id_ = id;}
	
	void set_stamp(double stamp) {stamp_ = stamp;}
	
	void set_rgb_raw(const cv::Mat & rgbraw) {rgb_raw_ = rgbraw;}
	
	void set_depth_raw(const cv::Mat & depthraw) {depth_raw_ = depthraw;}
	
	void set_camera_model(const CameraModel & cameramodel) {camera_models_.clear(); camera_models_.push_back(cameramodel);}
	
	void set_camera_models(const std::vector<CameraModel> & cameramodels) {camera_models_ = cameramodels;}
	
	void set_features(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
	{
		keypoints_ = keypoints;
		descriptors_ = descriptors;
	}

	void set_ground_truth(const Transform & pose) {groundTruth_ = pose;}
	
	
private:
	int id_;
	double stamp_;

	cv::Mat rgb_compressed_;          // compressed rgb image
	cv::Mat depth_compressed_;   // compressed depth image

	cv::Mat rgb_raw_;          // CV_8UC1 or CV_8UC3
	cv::Mat depth_raw_;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1

	std::vector<CameraModel> camera_models_;

	// features
	std::vector<cv::KeyPoint> keypoints_;
	cv::Mat descriptors_;

	Transform groundTruth_;
};

}

#endif /* SENSORDATA_H_ */